//============================================================================
// Name        : rscj.cpp
// Author      : Bob
// Version     :0
// Copyright   : Your copyright notice
// Description : your mom
//============================================================================


#include "stdafx.h"
#include <vector>
#include <ctime>


using namespace std;

const long double pi=3.14159265358979323846264338327950288419716939937510582097494459230781640628620899862803482534211;
const long double tmin=0;
const long double tmax=500;
const long double imax=10;
const long double deltai=0.01;
 long double deltat=0.01;
const long double deltatmax=0.1;
const long double deltatmin=0.001;
const long double ynull=1;
const long double betac=2.5;
const long double abserror=0.01;
const long double relerror=0.01;
const long double omega=2.85;
const long double imicro=5.0;

long double phitemp=0;
long double utemp=0;

void write(vector<long double>& data1,vector<long double>& data2,vector<long double>& data3, int& n);
void write(vector<long double>& data1,vector<long double>& data2, int& n);
long double fkt(long double& dc1,long double& y,long double& yt);
long double fkt(long double& dc1,long double& t,long double& y,long double& yt);
long double U_t(long double& dc1, long double& Unull);
void rk4(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1);
void rk4_dynstep(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1);
void rk5_dynstep(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1);
void rk4_step(long double& h,long double& dc1,long double& t,long double& v,long double& phi);
long double integrate(vector<long double>& t,vector<long double>& v);


int main() {

	clock_t begin = clock();


	int n=2*(imax/deltai);

	vector<long double> I;
	vector<long double> U;
	I.resize(n);
	U.resize(n);
	I[0]=0;
	long double temp=0.0;
	U[0]=U_t(I[0],temp);
	for(int i=1;i<n/2;i++){
		I[i]=I[i-1]+deltai;
		U[i]=U_t(I[i],U[i-1]);
	}
	for(int i=n/2;i<n;i++){
			I[i]=I[i-1]-deltai;
			U[i]=U_t(I[i],U[i-1]);
		}

	write(I,U,n);

	clock_t end = clock();
	double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
	cout<<elapsed_secs<<endl;
	char i;
	cin>>i;

	return elapsed_secs;
}

long double U_t(long double& dc1, long double& Unull){
	int n=(tmax-tmin)/deltat;
		vector<long double> t;
		vector<long double> v;
		vector<long double> phi;
		t.resize(1);
		v.resize(1);
		phi.resize(1);
		t.reserve(n);
		v.reserve(n);
		phi.reserve(n);
		t[0]=tmin;
		v[0]=Unull;
		phi[0]=0;
		

		//rk4(t,v,phi,deltat,dc1);
		rk4_dynstep(t,v,phi,deltat,dc1);
		//rk5_dynstep(t,v,phi,deltat,dc1);

			
	return integrate(t,v);

}


void write(vector<long double>& data1,vector<long double>& data2,vector<long double>& data3, int& n){
	fstream datei;
	datei.open("data.txt", ios::out);
	for(int i=0;i<n;i++){
		datei<<data1[i]<<" "<<data2[i]<<" "<<data3[i]<<endl;
	}
	datei.close();
}

void write(vector<long double>& data1,vector<long double>& data2, int& n){
	fstream datei;
	datei.open("data.txt", ios::out);
	for(int i=0;i<n;i++){
		datei<<data1[i]<<" "<<data2[i]<<endl;
	}
	datei.close();
}



long double fkt(long double& dc1,long double& y,long double& yt){
	cout<<dc1<<" "<<y<<" "<<yt<<endl;
	return ((1.0/betac)*(dc1-y-sin(yt)));
}
long double fkt(long double& dc1,long double& t,long double& y,long double& yt){
	return ((1.0/betac)*(dc1+imicro*sin(omega*t)-y-sin(yt)));
}
void rk4(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1){
//long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
//		long double t0=0;
//		int i=0;
//		while(t.back()<tmax){
//			v0=v.back();
//			phi0=phi.back();
//			dphi0=v0;
//			dv0=fkt(dc1,t0,v0,phi0);
//			phia=phi0+h/2.0*dphi0;
//			va=v0+h/2.0*dv0;
//			t0+=h/2.0;
//			dphia=va;
//			dva=fkt(dc1,t0,va,phia);
//			phib=phi0+h/2.0*dphia;
//			vb=v0+h/2.0*dva;
//			dphib=vb;
//			dvb=fkt(dc1,t0,vb,phib);
//			phic=phi0+h*dphib;
//			t0+=h/2.0;
//			vc=v0+h*dvb;
//			dphic=vc;
//			dvc=fkt(dc1,t0,vc,phic);
//			++i;
//			t.push_back(t.back()+h);
//			v.push_back(v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc));
//			phi.push_back(phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic));
//		}

			long double ttemp,vtemp,phitemp=0;
			while(t.back()<tmax){
				ttemp=t.back();
				vtemp=v.back();
				phitemp=phi.back();
				rk4_step(h,dc1,ttemp,vtemp,phitemp);
				t.push_back(ttemp+h);
				v.push_back(vtemp);
				phi.push_back(phitemp);
			}

}

void rk4_dynstep(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1){
	long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
	long double vtemp,vtemp2,vtemp22=0;
	long double phitemp,phitemp2,phitemp22=0;
	long double verror,phierror=0.0;
	long double t0=0;
	long double scale=0;
	bool err=true;
	long double h2=0;
	long double ttemp=0;


			int i=0;
			while(t.back()<tmax){
				err=true;
				i=0;
				t0=t.back();
				v0=v.back();
				phi0=phi.back();
				dphi0=v0;
				dv0=fkt(dc1,t0,v0,phi0);//first step in runge kutta can be reused

				while(err){
					h2=2.0*h;
					vtemp=v0;
					phitemp=phi0;
					ttemp=t0;
					rk4_step(h2,dc1,ttemp,vtemp,phitemp);
					vtemp2=v0;
					phitemp2=phi0;
					rk4_step(h,dc1,ttemp,vtemp2,phitemp2);
					vtemp22=vtemp2;
					phitemp22=phitemp2;
					ttemp+=h;
					rk4_step(h,dc1,ttemp,vtemp22,phitemp22);


				//t0=t.back();
				//	if(i==0){	//first run =>has to be calculated
				//////begin steping with double stepsize
				//phia=phi0+h*dphi0;
				//va=v0+h*dv0;
				//dphia=va;
				//t0+=h;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h*dphia;
				//vb=v0+h*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+2.0*h*dphib;
				//vc=v0+2.0*h*dvb;
				//t0+=h;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp=v0+h/3.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp=phi0+h/3.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving result for double stepsize
				//}else{	//not the first run can be taken from the last run
				//	vtemp=vtemp2;
				//	phitemp=phitemp2;
				//}
				////begin stepping for normal stepsize
				//phia=phi0+h/2.0*dphi0;	//dphi0 can be reused
				//va=v0+h/2.0*dv0;		//dv0 can also be reused
				//t0=t.back()+h/2.0;
				//dphia=va;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h/2.0*dphia;
				//vb=v0+h/2.0*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+h*dphib;
				//vc=v0+h*dvb;
				//t0+=h/2.0;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp2=v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp2=phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving results for first step with normal stepsize

				//////begin second step with normal stepsize
				//////v0 equals vtemp2 =>dphi0 equals vtemp2
				//////phi0 equals phitemp2
				//dv0=fkt(dc1,t0,vtemp2,phitemp2);
				//phia=phitemp2+h/2.0*vtemp2;	
				//va=vtemp2+h/2.0*dv0;
				//t0+=h/2.0;
				//dphia=va;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h/2.0*dphia;
				//vb=v0+h/2.0*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+h*dphib;
				//t0+=h/2.0;
				//vc=v0+h*dvb;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp22=v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp22=phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving results for second step with normal stepsize
				//				////calculating error
				//
				phierror=(fabs(phitemp22-phitemp));
				scale=phierror/(abserror+max(phitemp22,phitemp)*relerror);
				
				if(scale<1){
					err=false;
					
				}else{
				h=0.9*(h*(1.0/scale));
				}
				
				++i;
				}//end while(err)
				
				t.push_back(t.back()+h);
				v.push_back(vtemp2);
				phi.push_back(phitemp2);//saving first step
				
				t.push_back(t.back()+h);
				v.push_back(vtemp22);
				phi.push_back(phitemp22);//saving second step

				if((h<deltatmax) && (scale<0.9)&&(scale!=0)){
				h=0.9*(h*(1.0/scale));
				}else if((h<deltatmax)&&(scale==0)){
					h=h*2.0;
				}
				//cout<<dc1<<" "<<h<<" "<<phierror<<" "<<scale<<" "<<i<<endl;

			}
}

void rk5_dynstep(vector<long double>& t,vector<long double>& v,vector<long double>& phi,long double& h,long double& dc1){
	long double c[7]={0,1.0/5.0,3.0/10.0,4.0/5.0,8.0/9.0,1.0,1.0};
	long double b[7]={35.0/384.0,0.0,500.0/1113.0,125.0/192.0,-2187.0/6784.0,11.0/84.0,0.0};
	long double b2[7]={5179.0/57600.0,0.0,7571.0/16695.0,393.0/640.0,-92097.0/339200.0,187.0/2100.0,1.0/40.0};
	long double a[7][6]={
		{0,0,0,0,0,0},
		{1.0/5.0,0,0,0,0,0},
		{3.0/40.0,9.0/40.0,0,0,0,0},
		{44.0/45.0,-56.0/15.0,32.0/9.0,0,0,0},
		{19372.0/6561.0,-25360.0/2187.0,64448.0/6561.0,-212.0/729.0,0,0},
		{9017.0/3168.0,-355.0/33.0,46732.0/5247.0,49.0/176.0,-5103.0/18656.0,0},
		{35.0/384.0,0,500.0/1113.0,125.0/192.0,-2187.0/6784.0,11.0/84.0}
	};
	long double k[2][7]={}; //where k[0][0] is v[i] and k[0][6] is v[i+1];k[1][0] is phi[i] and k[1][6] is phi[i+1]  
	long double ktemp[2]={0};
	long double k0[2]={0};
	long double k5[2]={0};
	long double k4[2]={0};
	long double t0=0;
	long double ttemp=0;
	bool stepping=true;
	long double error=1;
	long double scale=abserror;
	
	while(t0<tmax){
	k0[0]=v.back();
	k0[1]=phi.back();
	t0=t.back();
	//while(stepping){
	for(int i=0;i<7;++i){
		ktemp[0]=k0[0];
		ktemp[1]=k0[1];
		
		for(int j=0;j<i;++j){
		ktemp[0]+=a[i][j]*k[0][j];
		ktemp[1]+=a[i][j]*k[1][j];
		//cout<<ktemp[0]<<endl;
		
		}
		
		ttemp=(t0+c[i]*h);
		k[0][i]=h*fkt(dc1,ttemp,ktemp[0],ktemp[1]);
		k[1][i]=h*k[0][i];
		//cout<<k[0][i]<<endl;

	}

	k5[0]=k0[0];
	k5[1]=k0[1];
	for(int i=0;i<6;++i){
		k5[0]+=b[i]*k[0][i];
		k5[1]+=b[i]*k[1][i];
	
	}
	//cout<<k5[0]<<endl;
	//k4[0]=k0[0];
	//k4[1]=k0[1];
	//for(int i=0;i<7;++i){
	//	k4[0]+=b2[i]*k[0][i];
	//	k4[1]+=b2[i]*k[1][i];
	//}
	//scale+=max(k5[1],k4[1])*relerror;
	////cout<<k5[1]<<" "<<k4[1]<<" "<<k5[0]<<" "<<k4[0]<<endl;
	//if(scale!=0){
	//	error=sqrt((pow((k5[0]-k4[0])/scale,2)+pow((k5[1]-k4[1])/scale,2))/2.0);
	//}else{
	//	error=0;
	//}
	//if(error<1){
	//	stepping=false;
	//}else if(h>deltatmin){
	////h=0.9*h*pow((1/error),(long double)0.5);
	//stepping=false;
	//}
	//

	//}//end while stepping

	t.push_back(t0+h);
	v.push_back(k5[0]);
	phi.push_back(k5[1]);

	/*if(error!=0 && h<deltatmax && error<0.9){
	h=0.9*h*pow((1/error),(long double)0.2);
	}else if(error==0 && h<deltatmax){
		h=h*2.0;
	}*/
	//long double temp=(k5[0]-k4[0]);
	//cout<<dc1<<" "<<h<<" "<<error<<" "<<scale<<" "<<endl;
	}//end while(t0<tmax)

}

long double integrate(vector<long double>& t,vector<long double>& v){
	long double U=0;
	int n=v.size();
	/*for(int i=0;i<n;i++){
			U=U+v[i];
		}
			U=U/n;*/
	for(int i=0;i<n-1;++i){
		U+=(t[i+1]-t[i])*v[i];
	}
	U=U/tmax;
	return U;
}

void rk4_step(long double& h,long double& dc1,long double& t,long double& v,long double& phi){
	long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
			long double t0=t;
				v0=v;
				phi0=phi;
				dphi0=v0;
				dv0=fkt(dc1,t0,v0,phi0);
				phia=phi0+h/2.0*dphi0;
				va=v0+h/2.0*dv0;
				t0+=h/2.0;
				dphia=va;
				dva=fkt(dc1,t0,va,phia);
				phib=phi0+h/2.0*dphia;
				vb=v0+h/2.0*dva;
				dphib=vb;
				dvb=fkt(dc1,t0,vb,phib);
				phic=phi0+h*dphib;
				t0+=h/2.0;
				vc=v0+h*dvb;
				dphic=vc;
				dvc=fkt(dc1,t0,vc,phic);
				
				v=(v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc));
				phi=(phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic));
			
}
